package crtl;

/**
 *
 * @author meraner marcel
 */
public abstract class ProportionalCtrl extends EPuckCtrl {

	public ProportionalCtrl() {
		super();
	}
	
	@Override
	protected abstract boolean finished();

	protected abstract double[] getConstants();

	protected abstract double[][] getControllMatrix();

	protected abstract double[] fetchAndPrepareSensorValues();

	@Override
	protected Speed navigate() {
		double[] a = new double[2];

		double s[] = fetchAndPrepareSensorValues();
		double c[] = getConstants();
		double k[][] = getControllMatrix();

		if ((s == null) || (c == null) || (k == null)) {
			return null;
		}
		
		// Matrix multiplication
		for (int i = 0; i < s.length; i++) {
			a[0] += k[0][i] * s[i] + c[0];
			a[1] += k[1][i] * s[i] + c[1];
		}

		double left = a[0];
		double right = a[1];

		// Normalize speedvalues between -1 and 1
		if ((left > 1) || (right > 1)) {
			double max = left;
			if (right > max) {
				max = right;
			}
			left /= max;
			right /= max;
		}
		if ((left < -1) || (right < -1)) {
			double min = left;
			if (right < min) {
				min = right;
			}
			left /= min;
			right /= min;
		}

		try {
			Speed speed = new Speed(left, right);
			return speed;

		} catch (SetSpeedException ex) {
			return null;
		}

	}

}
